#include "followRightWall.h"

ros::Publisher moto_pub;
ros::Publisher moto_change_pub;

saint_nr_9::IRdata ir_data;
bool correct_distance = false;
bool turned = false;
int global_state = START;

float distF(float a , float b){
	if(a >= 0 && b >= 0)
		return fabs(a-b);
	if(a >= 0 && b < 0)
		return fabs(a-b);
	if(a <= 0 && b < 0)
		return fabs(a-b);
	if(a <= 0 && b >= 0)
		return fabs(a)+b;
		
	return -1; //How the hell did we get here???
}

int main(int argc, char **argv){
	ros::init(argc, argv, "WallFollower"); //Creates a node named "WallFollower"
	ros::NodeHandle n;
	ros::Subscriber state_sub = n.subscribe("State/Info", 1, state_update);
	moto_pub = n.advertise<saint_nr_9::MotCom>("/control/motor_control", 1);
	moto_change_pub = n.advertise<saint_nr_9::MotCorrect>("/control/correct_motor",1);
	//ros::Subscriber state_sub = n.subscribe("state_info",1, );
	ros::Subscriber ir_sub = n.subscribe("ir_info", 1, ir_callback);
	
  state_functions[STRAIGHT_WALL] = &state_straight_wall;
  state_functions[CORNER] = &state_corner;
  state_functions[PART_HOLE_FRONT] = &state_part_hole_front;
  state_functions[HOLE] = &state_hole;
  state_functions[FIND_WALL] = &state_find_wall;
  state_functions[PART_WALL_FRONT] = &state_part_wall_front;
  state_functions[PART_WALL_PART_HOLE] = &state_part_wall_part_hole;
	state_functions[TAG_HANDLE] = &state_tag_handle;
	state_functions[TAG_PROCESSING] = &state_processing_handle;
	state_functions[START] = &state_start;
	state_functions[FIND_FIRST_WALL] = &state_find_first_wall;
	state_functions[WOWOWO] = &state_wowowo;
	state_functions[WOWOWO_TURN] = &state_wowowo_turn;		
	//moto_command.command = moto_command.MOVE_FORWARD;
	ros::Rate loop_rate(100);
	//sleep(5);
	//moto_pub.publish(moto_command);
	printf("Let's go... Move forward\n");

  while(ros::ok())
  {
  	//state_functions[state->data]();
  	if (global_state == STRAIGHT_WALL)
  	{
  	  
  	  state_functions[STRAIGHT_WALL]();
  	}
		//moto_pub.publish(moto_command);
		ros::spinOnce();
    loop_rate.sleep();
	} 
	
	
}




void ir_callback(const saint_nr_9::IRdata::ConstPtr& data){
	ir_data = *data;
}

saint_nr_9::MotCom calculate_necessary_angle_change(double white_reading, double orange_reading)
{
	saint_nr_9::MotCom moto_command;	
	double alpha = 2*atan(fabs(white_reading-orange_reading)/((double)DISTANCE_BETWEEN_SENSORS));
  if (white_reading > orange_reading) {
			  moto_command.command = TURN_RIGHT;
			  moto_command.dist_or_angle = alpha;
	} 
	else {
			  moto_command.command = TURN_LEFT;
			  moto_command.dist_or_angle = alpha;
	}
	return moto_command;
}

void state_straight_wall()
{
  //printf("State Straight Wall \n ");
  saint_nr_9::MotCom moto_command; 
  moto_command.command = MOVE_FORWARD_CORRECT;
  //moto_command.speed = SPEED_WALL;
  //printf("IR_Distance: %f, IR-Difference %f\n", ((ir_data.orange+ir_data.white)/2.0),  fabs(ir_data.orange-ir_data.white));
  saint_nr_9::MotCorrect correct;
  correct.left = 0;
  correct.right = 0;
  double closest_ir = (ir_data.red < ir_data.yellow?ir_data.red: ir_data.yellow) ;
  double slow_down = (closest_ir/(double)MAX_CORRECT_FRONT<1?closest_ir/(double)MAX_CORRECT_FRONT:1);
  slow_down = (slow_down < 0.5? 0.5: slow_down);
  moto_command.speed = SPEED_WALL*slow_down;
  double alpha = atan(fabs(ir_data.orange-ir_data.white)/((double)DISTANCE_BETWEEN_SENSORS))*57.2957795;
  
  if (ir_data.white < MIN_MIDDLE_DISTANCE_TO_THE_SIDE && ((alpha < MAX_ROBO_WINKEL && ir_data.white > ir_data.orange) || (ir_data.white < ir_data.orange)))
  {
    double value = ir_data.white;
    value = (value < 0? 0:value);
    correct.left = fabs(value-DISTANCE_TO_THE_WALL)/MAX_OFF_OF_DISTANCE;
    //printf("LEFT SLOW DOWN %f\n", correct.left);
    correct.left = (correct.left > 0.2? 0.2: correct.left);
    correct.right = (correct.right > 0.1? 0.1: correct.right);
  } 
  else if (ir_data.white > MAX_MIDDLE_DISTANCE_TO_THE_SIDE  && ((alpha < MAX_ROBO_WINKEL && ir_data.white < ir_data.orange) || (ir_data.white > ir_data.orange)))
  {
    correct.right = fabs(ir_data.white-DISTANCE_TO_THE_WALL)/MAX_OFF_OF_DISTANCE;
    //printf("RIGHT SLOW DOWN %f\n", correct.right);
    correct.left = (correct.left > 0.15? 0.15: correct.left);
    correct.right = (correct.right > 0.1? 0.1: correct.right);
  }
  else
  {
    double CORRECTION_SLOWDOWN = 0.5; //try 0.3
    double front = (ir_data.white < 0? 0:ir_data.white);
    double back = (ir_data.orange < 0? 0:ir_data.orange);
    if (front > back){
      correct.right = 1 - (back / front);
      correct.right *= CORRECTION_SLOWDOWN;
    }else{
      correct.left = 1 - (front / back);
      correct.left *= CORRECTION_SLOWDOWN;
    }
   }
  
  moto_change_pub.publish(correct);
  //printf("State Straight Wall \n ");
	moto_pub.publish(moto_command);
	//printf("State Straight Wall \n ");
}

void state_processing_handle()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = STOP;
  moto_pub.publish(moto_command);   
}

void state_tag_handle ()
{
  saint_nr_9::MotCom moto_command;
  //moto_command.command = MOVE_FORWARD_CORRECT;
  moto_command.command = MOVE_FORWARD;
  moto_command.speed = -SPEED_SLOW;
  //printf("IR_Distance: %f, IR-Difference %f\n", ((ir_data.orange+ir_data.white)/2.0),  fabs(ir_data.orange-ir_data.white));
  /*
	saint_nr_9::MotCorrect correct;
  correct.left = 0;
  correct.right = 0;
  
  double alpha = atan(fabs(ir_data.orange-ir_data.white)/((double)DISTANCE_BETWEEN_SENSORS))*57.2957795;
  
  if (ir_data.orange < MIN_MIDDLE_DISTANCE_TO_THE_SIDE && ((alpha < MAX_ROBO_WINKEL && ir_data.orange > ir_data.white) || (ir_data.orange < ir_data.white)))
  {
    double value = ir_data.orange;
    value = (value < 0? 0:value);
    correct.left = fabs(value-DISTANCE_TO_THE_WALL)/MAX_OFF_OF_DISTANCE;
    //printf("LEFT SLOW DOWN %f\n", correct.left);
    correct.right = (correct.right > 0.1? 0.1: correct.left);
    correct.left = (correct.left > 0.15? 0.15: correct.right);
  } 
  else if (ir_data.orange > MAX_MIDDLE_DISTANCE_TO_THE_SIDE  && ((alpha < MAX_ROBO_WINKEL && ir_data.orange < ir_data.white) || (ir_data.orange > ir_data.white)))
  {
    correct.right = fabs(ir_data.white-DISTANCE_TO_THE_WALL)/MAX_OFF_OF_DISTANCE;
    //printf("RIGHT SLOW DOWN %f\n", correct.right);
    correct.left = (correct.left > 0.15? 0.15: correct.left);
    correct.right = (correct.right > 0.1? 0.1: correct.right);
  }
  else
  {
    double CORRECTION_SLOWDOWN = 0.5; //try 0.3
    double front = (ir_data.orange < 0? 0:ir_data.orange);
    double back = (ir_data.white < 0? 0:ir_data.white);
    if (front > back){
      correct.right = 1 - (back / front);
      correct.right *= CORRECTION_SLOWDOWN;
    }
    else
    {
      correct.left = 1 - (front / back);
      correct.left *= CORRECTION_SLOWDOWN;
    }
  }
  moto_change_pub.publish(correct);*/
	moto_pub.publish(moto_command);
}

void state_corner()
{
  printf("State Corner\n");
  saint_nr_9::MotCom moto_command;
  moto_command.command = TURN_LEFT;
  moto_command.dist_or_angle = 90;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
} 

void state_part_hole_front()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = MOVE_FORWARD;
  moto_command.speed = SPEED_SLOW;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}

void state_hole()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = TURN_RIGHT;
  moto_command.dist_or_angle = 90;
  moto_command.speed = SPEED_TURN;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}

void state_find_wall()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = MOVE_FORWARD;
  moto_command.speed = SPEED_SLOW;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}

void state_part_wall_front()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = MOVE_FORWARD;
  moto_command.speed = SPEED_SLOW;
  moto_pub.publish(moto_command);
}

void state_part_wall_part_hole()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = MOVE_FORWARD;
  moto_command.speed = SPEED_SLOW;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}

void state_start()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = STOP;
  moto_pub.publish(moto_command);  
}

void state_find_first_wall()
{
  printf("State Find First Wall\n");
  saint_nr_9::MotCom moto_command;
  moto_command.command = MOVE_FORWARD;
  moto_command.speed = SPEED_SLOW;
  printf("State Find First Wall\n");    
  moto_pub.publish(moto_command);
  printf("State Find First Wall\n");  
}

void state_wowowo()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = MOVE_BACKWARD_DIST;
  moto_command.speed = SPEED_SLOW;
  moto_command.dist_or_angle = 0.05;  
  moto_pub.publish(moto_command);  
}

void state_wowowo_turn()
{
  saint_nr_9::MotCom moto_command;
  moto_command.command = TURN_LEFT;
  moto_command.dist_or_angle = 90;
  moto_command.speed = SPEED_TURN;
  //for(int i = 0; i < 10; i++)
  moto_pub.publish(moto_command);
}


void state_update(const std_msgs::Int32::ConstPtr& state)
{
  global_state = state->data;
  printf("Input state: %d\n", state->data);
  if (state->data < 13) {
    state_functions[state->data]();
  }
}

